#!/usr/bin/env python3

import rospy
import actionlib

from cre.msg import CreActionAction, CreActionGoal, CreActionResult, CreActionFeedback


class MyActionServer(object):
    def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, CreActionAction, execute_cb=self.executeCB,
                                                auto_start=False)
        self._as.start()
        self._feedback = CreActionFeedback()
        self._result = CreActionResult()

    def executeCB(self, goal):
        r = rospy.Rate(1)
        success = True
        # 执行动作
        for i in range(1, 11):
            if self._as.is_preempt_requested():
                rospy.loginfo('{}: Preempted'.format(self._action_name))
                self._as.set_preempted()
                success = False
                break

            self._feedback.progress = i * 10
            rospy.loginfo('{}: Executing, progress = {}'.format(self._action_name, self._feedback.progress))
            self._as.publish_feedback(self._feedback)

            r.sleep()

        # 发送结果
        if success:
            self._result.success = True
            rospy.loginfo('{}: Succeeded'.format(self._action_name))
            self._as.set_succeeded(self._result)


if __name__ == '__main__':
    rospy.init_node('my_server')
    server = MyActionServer('my_action')
    rospy.spin()
